#include "KS108.h"

#include "font.h"
//#include "twi.h"

byte KS108_y;
byte KS108_x;


void KS108_SetDataMode( byte rs )
{
  // D7 D6 D5 D4 D3 D2 D1 D0 | X 0 0 0 CS2 CS1 RW RS 
  if ( rs>0)
    rs = 1;

  Wire.beginTransmission( 0x52 ); 
  if ( KS108_y < 64 )
  {
    Wire.send( 4 | rs );       
  }
  else
  {
    Wire.send( 8 | rs );       
  }
  Wire.endTransmission();    
}

void KS108_SendData( byte data )
{
  Wire.beginTransmission( 0x53 ); 
  Wire.send( data );       
  Wire.endTransmission();      

  KS108_y++;

  if ( KS108_y == 64 )
  {
    KS108_Locate( KS108_x, 64 );
  }
  else if ( KS108_y >= 128 )
  {
    KS108_Locate( KS108_x+1, 0 );
  }
}

byte KS108_ReadData()
{
  KS108_y++;
  Wire.requestFrom(0x53,1);
  while (Wire.available() == false);
  return Wire.receive(); 
}



void KS108_Locate( byte x, byte y )
{
  KS108_x = x;
  KS108_y = y;

  KS108_SetDataMode( 0 );

  Wire.beginTransmission( 0x53 ); 
  Wire.send( 64 + ( y & 63 ) );       
  Wire.send( 128+32+16+8+ ( ( x >> 3 ) & 7 ) );       
  Wire.endTransmission();      

  KS108_SetDataMode( 1 );
}

void SelectBank( byte side, byte row)
{
  Wire.beginTransmission( 0x52 ); 
  Wire.send( (4<<side) );       
  Wire.endTransmission();    

  Wire.beginTransmission( 0x53 ); 
  Wire.send( 64 + ( 0 & 63 ) );       
  Wire.send( 128+32+16+8+ (row & 7 ) );       
  Wire.endTransmission();      

  Wire.beginTransmission( 0x52 ); 
  Wire.send( (4<<side) | 1 );       
  Wire.endTransmission();      
}

void KS108_Draw8bitPicRow( byte row, byte *pic )
{
  char i,k;

  for ( byte side=0;side<2;side++)
  {
    SelectBank( side, row);

    for(k=0;k<2;k++)
    {    

      Wire.beginTransmission( 0x53 ); 
      for (i=0;i<32;i++)  
      {
        Wire.send( *pic++ );       
      }
      Wire.endTransmission();      
    }
  }
}


void KS108_Draw8bitPicRowPgm( byte row, byte *pic )
{
  char i,k;

  for ( byte side=0;side<2;side++)
  {
    SelectBank( side, row);

    for(k=0;k<2;k++)
    {   
      Wire.beginTransmission( 0x53 ); 
      for (i=0;i<32;i++)  
      {
        Wire.send( (uint8_t)pgm_read_byte( pic++ ) );       
      }
      Wire.endTransmission();      
    }
  }
}

void KS108_DrawPic( byte *pic )
{
  for(char j=0;j<8;j++)
  {
    KS108_Draw8bitPicRowPgm( j, pic );
    pic+=256;
  }
}


void KS0108_PrintChar5x8( char c )
{
  if ( c == '\r' )
  {
    KS108_Locate( KS108_x+8, 0 );
    return;
  }

  const char *data = &font5x8[ (c -' ') * 5];

  for(byte i=0;i<5;i++)
  {
    KS108_SendData( pgm_read_byte(data++) );
  }
  KS108_SendData( 0 ); 
}


void print_binary(int v )
{
  for( byte i=0;i<8;i++)
  {
    if (v & 128 )
    {
      Serial.print("1");
    }
    else
    {
      Serial.print("0");
    }
    v<<=1;
  }
}

void KS108_Send4bytes( byte *back )
{
  Wire.beginTransmission( 0x53 ); 
  for(byte i=0;i<4;i++)
  {
    Wire.send( back[i] );       
    KS108_y++;
    if ( KS108_y == 64 )
    {
      Wire.endTransmission();      

      Wire.beginTransmission( 0x52 ); 
      Wire.send( 8 | 1 );   
      Wire.endTransmission();          

      Wire.beginTransmission( 0x53 ); 
    }
  }
  Wire.endTransmission();        
}

#define SIGNEDSHIFT( a, b ) ((b>0)?(a<<b):(a>>-b))

void KS108_Read4bytes( const byte *data, byte *back, char offset )
{
  byte mask  = ~SIGNEDSHIFT( 0b00111111, offset);

  // read 5 bytes from lcd  
  Wire.requestFrom(0x53,5);

  for(byte i=0;i<3;i++)
  {
    byte d = pgm_read_byte(data++) >> 2;
    back[i] = SIGNEDSHIFT( d, offset );
  }
  back[3] = 0;

  //while (Wire.available() == 0 );
  Wire.receive(); 

  for(byte i=0;i<4;i++)
  {
    //while (Wire.available() == 0 );
    back[i] |= Wire.receive() & mask; 
  }
  KS108_y+=5;
}

//
// 1473 ms  top 16chars x 50
// 2403 ms  top 16chars x 50
// 2903 ms  top 16chars x 50  ( with read data )
// 2066 ms  top 16chars x 50  ( with read data returning 0 )
// 1706 ms  top 16chars x 50  ( with read data returning 0 and using wire)
// 2804 ms  full 16chars x 50 ( with read data returning 0 and using wire)
// 4137 ms  full 16chars x 50 with readdata
void KS0108_PrintChar3x6( char c )
{
  if ( c == '\r' )
  {
    KS108_Locate( KS108_x+6, 0 );
    return;
  }

  byte back[4];

  const byte *data = (const byte *)&au8FontSystem3x6[ (c -' ') * 3];

  char offset = ( KS108_x & 7 );

  KS108_Locate( KS108_x, KS108_y );  

  KS108_Read4bytes( data, back, offset );
  KS108_Locate( KS108_x, KS108_y - 5 );

  KS108_Send4bytes( back );

  if ( offset > 3 )
  {
    KS108_Locate( KS108_x + 6, KS108_y - 4 );

    KS108_Read4bytes( data, back, offset - 8);
    KS108_Locate( KS108_x, KS108_y - 5 );

    KS108_Send4bytes( back );

    KS108_Locate( KS108_x - 6, KS108_y );
  }

}

void KS0108_PrintChar3x6_slow( char c )
{
  if ( c == '\r' )
  {
    KS108_Locate( KS108_x+6, 0 );
    return;
  }

  char back[4];

  //dummy read
  KS108_Locate( KS108_x, KS108_y );
  KS108_ReadData();

  for(byte i=0;i<4;i++)
  {
    back[i] = KS108_ReadData();
  }

  KS108_Locate( KS108_x, KS108_y - 5 );

  const char *data = &au8FontSystem3x6[ (c -' ') * 3];

  byte offset = ( KS108_x & 7 );
  byte mask = ~(0b00111111 << offset);

  for(byte i=0;i<3;i++)
  {
    KS108_SendData( (( ( pgm_read_byte(data++) >> 2 ) << offset ) & ~mask) | ( back[i] & mask ) );
  }
  KS108_SendData( back[3] & mask );

  if ( offset > 3 )
  {
    data -=3;

    offset = 8 - offset;

    mask = ~(0b00111111 >> offset);

    KS108_Locate( KS108_x + 6, KS108_y - 4 );

    //dummy read
    KS108_ReadData();

    for(byte i=0;i<4;i++)
    {
      back[i] = KS108_ReadData();
    }

    KS108_Locate( KS108_x, KS108_y - 5 );

    for(byte i=0;i<3;i++)
    {
      KS108_SendData( ( ( ( pgm_read_byte(data++) >> 2 ) >> offset) & ( ~mask ) ) | ( back[i] & mask ) );
    }
    KS108_SendData( back[3] & mask );

    KS108_Locate( KS108_x - 6, KS108_y );
  }

}

void KS0108_PrintStr3x6( char * str )
{
  for (;*str!=0;str++)
    KS0108_PrintChar3x6( *str );
}


void KS108_Cls()
{
  char i,j,k;

  for(j=0;j<8;j++)
  {
    Wire.beginTransmission( 0x52 ); 
    Wire.send( 8 | 4 );       
    Wire.endTransmission();    

    Wire.beginTransmission( 0x53 ); 
    Wire.send( 64 + ( 0 & 63 ) );       
    Wire.send( 128+32+16+8+ (j & 7 ) );       
    Wire.endTransmission();      

    Wire.beginTransmission( 0x52 ); 
    Wire.send( 8 | 4 | 1 );       
    Wire.endTransmission();    

    for(k=0;k<2;k++)
    {
      Wire.beginTransmission( 0x53 ); 
      for (i=0;i<32;i++)  
      {
        Wire.send( 0 );       
      }
      Wire.endTransmission();      
    }
  }

  KS108_Locate( 0, 0 );

}

void KS108_Init()
{
  Wire.beginTransmission( 0x52 ); 
  Wire.send( 4 | 8 );       
  Wire.endTransmission();    

  Wire.beginTransmission( 0x53 ); 
  Wire.send( 63 );  //display on
  Wire.send( 128+64 ); // z = 0
  Wire.endTransmission();      

  KS108_SetDataMode( 1 );

}

// D7 D6 D5 D4 D3 D2 D1 D0 | X 0 0 0 CS2 CS1 RW RS 
void KS108_SetPixel(byte x, byte y, PIXOP pval)
{
  KS108_Locate(x, y);

  //dummy read
  KS108_ReadData();

  byte b = KS108_ReadData();

  KS108_Locate(x, y);

  switch( pval )
  {
  case SET:    
    b |= ( 1 << ( x & 7 ) ); 
    break;
  case CLEAR:  
    b &= ~( 1 << ( x & 7 ) ); 
    break;
  }

  KS108_SendData( b );   
}


void KS108_lineBresenham( char x0, char y0, char x1, char y1, PIXOP pval )
{
  char dy = y1 - y0;
  char dx = x1 - x0;
  char stepx, stepy;

  if (dy < 0) 
  { 
    dy = -dy;  
    stepy = -1; 
  } 
  else 
  { 
    stepy = 1; 
  }
  if (dx < 0) 
  { 
    dx = -dx;  
    stepx = -1; 
  } 
  else 
  { 
    stepx = 1; 
  }
  dy <<= 1;                                                  // dy is now 2*dy
  dx <<= 1;                                                  // dx is now 2*dx

  KS108_SetPixel(x0, y0, pval);

  if (dx > dy) 
  {
    char fraction = dy - (dx >> 1);                         // same as 2*dy - dx
    while (x0 != x1) 
    {
      if (fraction >= 0) 
      {
        y0 += stepy;
        fraction -= dx;                                // same as fraction -= 2*dx
      }

      x0 += stepx;
      fraction += dy;                                    // same as fraction -= 2*dy
      KS108_SetPixel(x0, y0, pval);
    }
  } 
  else 
  {
    char fraction = dx - (dy >> 1);
    while (y0 != y1) 
    {
      if (fraction >= 0) 
      {
        x0 += stepx;
        fraction -= dy;
      }

      y0 += stepy;
      fraction += dx;
      KS108_SetPixel(x0, y0, pval);
    }
  }
}

void KS108_GetPicFromSerial()
{
  KS108_Locate( 0, 0 );
  for(int b=0;b<128*8;b++)
  {
    while( Serial.available() == false )
    {
    }
    KS108_SendData( Serial.read() );
  }
}

/*
void KS108_BenchmarkPrintChar3x6()
 {
 int start;
 
 KS108_Cls();
 start = millis() ;
 
 for(int j=0;j<50;j++)
 {
 //KS108_DrawPic( konami );
 KS108_Locate( j, 0 );
 
 for(int i=0;i<128/4;i++)
 KS0108_PrintChar3x6('A' + i );
 }
 
 Serial.print(millis() - start) ;
 Serial.println(" msec (1000 calls)") ;
 
 KS108_Cls();
 start = millis() ;
 
 for(int j=0;j<50;j++)
 {
 //KS108_DrawPic( konami );
 
 KS108_Locate( j, 0 );
 
 for(int i=0;i<128/4;i++)
 KS0108_PrintChar3x6_slow('A' + i );
 }
 
 Serial.print(millis() - start) ;
 Serial.println(" slow msec (1000 calls)") ;
 
 }
 
 
 void KS108_BenchmarkDrawPic()
 {
 int start = millis() ;
 for(char i=0; i<10; i++ )
 {
 KS108_DrawPic( konami );    
 KS108_DrawPic( KnightLore );
 KS108_DrawPic( konami );    
 KS108_DrawPic( KnightLore );
 KS108_DrawPic( konami );    
 KS108_DrawPic( KnightLore );
 KS108_DrawPic( konami );    
 KS108_DrawPic( KnightLore );
 KS108_DrawPic( konami );    
 KS108_DrawPic( KnightLore );
 }
 Serial.print(millis() - start) ;
 Serial.println(" msec (1000 calls)") ;  
 }
 */







